#include "ultra_band_wave_ladar/Sensor_mmRad.hpp"

enum CommdMode
{
  AlarmLimit,
  MaxDist,    //单位为米
  MinDist,    //单位为米
  DelayTime,  //单位为秒
  BaudRate,
  ContinueScan,
  OnceScan,
  GetParam,
  GetParam_,  // ASCII格式
};

Sensor_mmRad::Sensor_mmRad()
{
  m_alarmDistance = 0;
  distance = 0;
  mag = 0;
  angle = 0;  //如果有角度，就使用
}

Sensor_mmRad::~Sensor_mmRad()
{
}

void Sensor_mmRad::set_AlarmDistance(int Dist)
{
  // unit cm
  m_alarmDistance = Dist;
}

bool Sensor_mmRad::Init_mmRadComPort(std::string name)
{
  try
  {
    ser.setPort(name);
    ser.setBaudrate(115200);  //默认115200
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ser.setTimeout(to);
    ser.open();
  }
  catch (const std::exception& e)
  {
    ROS_ERROR_STREAM("Unbale to open port");
    return false;
  }

  if (ser.isOpen())
  {
    ROS_INFO_STREAM("Serial Port  Initialized");
  }
  else
  {
    return false;
  }
  return true;
}

void Sensor_mmRad::setScanData(std::vector<uint8_t> data)
{
  for (int i = 0; i < data.size(); i++)
  {
    reciveData.push_back(data[i]);
  }
  distance = comb16(reciveData[1], reciveData[2]);
  mag = comb16(reciveData[3], reciveData[4]);
  // angle = data;
}

//激活雷达,开始扫描
void Sensor_mmRad::mmRadScan_Start()
{
  set_CommandMode(5, 0);
}

uint16_t Sensor_mmRad::comb16(uint8_t low, uint8_t high)
{
  uint16_t newdata = 0;
  newdata = ((uint16_t)high << 8);
  newdata |= ((uint16_t)low);
}

void Sensor_mmRad::set_CommandMode(int type, int Var)
{
  uint8_t Temp = 0x00;
  switch (type)
  {
    case 0:
      Temp = 0xff | (Var / 10);
      Mode[2] = 0xA8;
      Mode[3] = Temp;
      Mode[4] = DataOXROperate(&Mode[0], 3);
      ser.write(&Mode[0], 6);
      ROS_INFO_STREAM("write:" << Mode);
      break;

    case 1:
      Temp = 0xff | (Var * 10);
      Mode[2] = 0xC1;
      Mode[3] = Temp;
      Mode[4] = DataOXROperate(&Mode[0], 3);
      ser.write(&Mode[0], 6);
      ROS_INFO_STREAM("write:" << Mode);
      break;

    case 2:
      Temp = 0xff | (Var * 10);
      Mode[2] = 0xC2;
      Mode[3] = Temp;
      Mode[4] = DataOXROperate(&Mode[0], 3);
      ser.write(&Mode[0], 6);
      ROS_INFO_STREAM("write:" << Mode);
      break;

    case 3:
      Temp = 0xff | Var;
      Mode[2] = 0xA6;
      Mode[3] = Temp;
      Mode[4] = DataOXROperate(&Mode[0], 3);
      ser.write(&Mode[0], 6);
      ROS_INFO_STREAM("write:" << Mode);
      break;

    case 4:  // Var 0=115200,1=57600,2=28400,3=19200,4=9600
      Temp = 0xff | Var;
      Mode[2] = 0xC8;
      Mode[3] = Temp;
      Mode[4] = DataOXROperate(&Mode[0], 3);
      ser.write(&Mode[0], 6);
      ROS_INFO_STREAM("write:" << Mode);
      break;

    case 5:
      Mode[2] = 0x80;
      Mode[3] = 0x0B;
      Mode[4] = 0x88;
      ser.write(&Mode[0], 6);
      ROS_INFO_STREAM("write:" << Mode);
      break;

    case 6:
      Mode[2] = 0x80;
      Mode[3] = 0x0C;
      Mode[4] = 0x8F;
      ser.write(&Mode[0], 6);
      ROS_INFO_STREAM("write:" << Mode);
      break;

    case 7:
      Mode[2] = 0x0D;
      Mode[3] = 0x01;
      Mode[4] = 0x0F;
      ser.write(&Mode[0], 6);
      ROS_INFO_STREAM("write:" << Mode);
      break;

    case 8:
      Mode[2] = 0x80;
      Mode[3] = 0x0F;
      Mode[4] = 0x8C;
      ser.write(&Mode[0], 6);
      ROS_INFO_STREAM("write:" << Mode);
      break;

    default:
      break;
  }
}

void Sensor_mmRad::CloseComPort()
{
  ser.close();
}

uint8_t Sensor_mmRad::DataOXROperate(uint8_t* pt, int num)
{
  uint8_t Data = 0x00;
  for (int i = 1; i < num + 1; i++)
  {
    Data = Data ^ pt[i];
  }
  return Data;
}

void Sensor_mmRad::get_mmRadComScanData()
{
  size_t n = ser.available();
  if (n)
  {
    ROS_INFO_STREAM("Reading from serial port\n");
    uint8_t buffer[1024];
    n = ser.read(buffer, n);
    for (int i = 0; i < n; i++)
    {
      std::cout << std::hex << (buffer[i] & 0xff) << " ";
    }
    distance = comb16(buffer[1], buffer[2]);
    mag = comb16(buffer[3], buffer[4]);
    // mmRad.mag = comb16(buffer[3],buffer[4]);
    // std::cout<<"Obstacle's position is "<<distance<<" cm"<<" ";
    // std::cout<<"Obstacle's magnitude is "<<mag<<" ";
    // std::cout<<std::endl;
  }
}

bool Sensor_mmRad::ObstacleScan()
{
  std_msgs::String result;
  if (distance != 0 && distance <= m_alarmDistance)
  {
    result.data = "Obstacle's distance:" + std::to_string(distance) + "; Obstacle's magnitude:" + std::to_string(mag);
    ROS_INFO_STREAM("mmRad_Sensor:" << result.data);
    return true;
  }
  return false;
}

uint16_t Sensor_mmRad::get_ActualDistance()
{
  return distance;
}
uint16_t Sensor_mmRad::get_ActualAmplitude()
{
  return mag;
}
uint16_t Sensor_mmRad::get_ActualAngle()
{
  return angle;
}